/**
  ******************************************************************************
  * File Name          : jaka_moveit_driver.cpp
  * Description       : jaka_moveit_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <jaka_driver/net_comm.h>

using namespace std;                                                          
int clitfd;                                       //文件描述符                     
struct sockaddr_in serv_addr;                     //目的服务端地址结构体                                  
unsigned char tcp_txd_buf[TCP_TX_BUF_MAX];                                          
unsigned char tcp_rxd_buf[TCP_RX_BUF_MAX];   

/* Private function prototypes -----------------------------------------------*/                                  
/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */                                                                          
static void FloatToByte(float floatNum, unsigned char *byteArry)              
{                                                                             
  unsigned char *pchar = (unsigned char *)&floatNum;                          
  char i;                                                                     
  for (i = 0; i < sizeof(float); i++)                                         
  {                                                                           
    *byteArry = *pchar;                                                       
    pchar++;                                                                  
    byteArry++;                                                               
  }                                                                           
}     
                                                                        
/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */                                                                          
static float ByteToFloat(unsigned char* byteArray)                            
{                                                                             
  return *((float*)byteArray);                                                
}                                                                             
                                                                         
/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */                                                                          
static float net_get_rxd(uint8_t index)                                              
{                                                                             
/* to do */                                                                   
  uint8_t x = index*DATA_WIDTH;                                               
  uint8_t byteArry[DATA_WIDTH];                                               
  for(int i = APP_NULL; i < DATA_WIDTH; i++)                                  
  {                                                                           
    byteArry[i] = tcp_rxd_buf[x+i];                                           
  }                                                                           
  return ByteToFloat(byteArry);                                               
/* end */                                                                     
}  
                                                                           
/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */                                                                          
static void net_set_txd(uint8_t index, float data)                                   
{                                                                             
/* to do */                                                                   
  uint8_t x = index*DATA_WIDTH;                                               
  uint8_t byteArry[DATA_WIDTH];                                               
  FloatToByte(data,byteArry);                                                 
  for(int i = APP_NULL; i < DATA_WIDTH; i++)                                  
  {                                                                           
    tcp_txd_buf[x+i] = byteArry[i];                                           
  }                                                                           
/* end */                                                                     
}  
                                                                           
/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */                                                                          
static uint8_t net_chkmsg(uint8_t msg[TCP_RX_BUF_MAX])                               
{                                                                             
/* to do */                                                                   
  uint32_t cs = APP_NULL;                                                     
  uint32_t tmp = APP_NULL;                                                    
  for(int i = APP_NULL; i< (TCP_RX_BUF_MAX-DATA_WIDTH); i++)                  
  {                                                                           
    tmp += msg[i];                                                            
  }                                                                           
  for(int j = APP_NULL; j < DATA_WIDTH; j++)                                  
  {                                                                           
    cs <<= 8;                                                                 
    cs += tcp_rxd_buf[NET_CHKS*DATA_WIDTH+3-j];                               
  }                                                                           
  if(cs==tmp)                                                                 
  {                                                                           
    return 1;                                                                 
  }                                                                           
  else                                                                        
  {                                                                           
    return 0;                                                                 
  }                                                                           
/* end */                                                                     
}   

/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */ 
unsigned int net_tcp_write(unsigned char cmd, unsigned char idx, unsigned char ste, float dtx[8])
{       
  int sdlen = 0;         
  unsigned char i;             
  unsigned int cs = APP_NULL;
                                                     
/* 填充发送帧的数据头 */                                              
  tcp_txd_buf[0] = '$';                                      
  tcp_txd_buf[1] = cmd;                              
  tcp_txd_buf[2] = idx;                              
  tcp_txd_buf[3] = ste; 
                             
/* 填充订阅的数据 */                                                
  net_set_txd(1, dtx[0]);                             
  net_set_txd(2, dtx[1]);                             
  net_set_txd(3, dtx[2]);                             
  net_set_txd(4, dtx[3]);                             
  net_set_txd(5, dtx[4]);                             
  net_set_txd(6, dtx[5]);                             
  net_set_txd(7, 0);                             
  net_set_txd(8, 0); 
                            
/* 计算和填充发送帧的校验码 */                                           
  for(int i = APP_NULL; i < (TCP_TX_BUF_MAX-DATA_WIDTH); i++)
  {                                                          
    cs+=tcp_txd_buf[i];                                      
  }                                                          
  tcp_txd_buf[TCP_TX_BUF_MAX-4] = (uint8_t)cs;               
  tcp_txd_buf[TCP_TX_BUF_MAX-3] = (uint8_t)(cs>>8);          
  tcp_txd_buf[TCP_TX_BUF_MAX-2] = (uint8_t)(cs>>16);         
  tcp_txd_buf[TCP_TX_BUF_MAX-1] = (uint8_t)(cs>>24);         
  sdlen = send(clitfd, tcp_txd_buf, TCP_TX_BUF_MAX, 0);  
  
  return sdlen;                                         
}                                                    

/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */ 
unsigned int net_tcp_read(unsigned char *cmd, unsigned char *idx, unsigned char *ste, float dtx[8])
{ 
  unsigned int rdlen = recv(clitfd, tcp_rxd_buf, TCP_RX_BUF_MAX, 0);
  if(rdlen>0 && net_chkmsg(tcp_rxd_buf))
  {
    *cmd = tcp_rxd_buf[1];
    *idx = tcp_rxd_buf[2];
    *ste = tcp_rxd_buf[3];
    dtx[0] = net_get_rxd(1);
    dtx[1] = net_get_rxd(2);
    dtx[2] = net_get_rxd(3);
    dtx[3] = net_get_rxd(4);
    dtx[4] = net_get_rxd(5);
    dtx[5] = net_get_rxd(6);
    dtx[6] = net_get_rxd(7);
    dtx[7] = net_get_rxd(8);
    //printf("rxd: %d %d %d %f %f %f %f %f %f %f %f\r\n",
    //        *cmd, *idx, *ste,
    //        dtx[0], dtx[1], dtx[2], dtx[3], dtx[4], dtx[5], dtx[6], dtx[7]
    //);
    return 1;
  }
  return 0;
}

/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */                                                                          
void net_tcp_open(void)                                                       
{                                                                             
    memset(&serv_addr,0,sizeof(serv_addr));                                   
    if((clitfd = socket(AF_INET,SOCK_STREAM,0)) == -1)  //创建套接字               
    {                                                                         
        cout<<"creat socket failed : "<<strerror(errno)<<endl;                
    }    
                                                               
    //将目的服务端的地址信息赋值给地址结构体                                                     
    serv_addr.sin_family = AF_INET;                                           
    serv_addr.sin_port = htons(8888);                                         
    serv_addr.sin_addr.s_addr = inet_addr("192.168.1.188");                   
    cout<<"try to connect x-smart ... "<<endl;                                
    //通过套接字发起连接请求，成功后clitfd套接字则表示此次成功的连接                                      
    if( connect(clitfd,(struct sockaddr*)& serv_addr,sizeof(serv_addr)) == -1)
    {                                                                         
        cout<<"connet failed : "<<strerror(errno)<<endl;                      
    }        
         
    int flags = fcntl(clitfd, F_GETFL);
    flags |= O_NONBLOCK;
    fcntl(clitfd, F_SETFL, flags);  
     
    cout<<"connect success !"<<endl;                                          
}  
                                                                           
/**                                                                           
  * @brief                                                                    
  * @param                                                                    
  * @retval                                                                   
  */                                                                          
void net_tcp_close(void)                                                      
{                                                                             
    close(clitfd);                                                            
} 

                                                                          
